#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using JigLibX.Geometry;
using JigLibX.Math;
#endregion

namespace JigLibX.Collision
{
    public class CollDetectBoxPlane : DetectFunctor
    {

        public CollDetectBoxPlane()
            : base("BoxPlane", (int)PrimitiveType.Box, (int)PrimitiveType.Plane)
        {
        }

        public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
        {
            if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
            {
                CollisionSkin skinSwap = info.Skin0;
                info.Skin0 = info.Skin1;
                info.Skin1 = skinSwap;
                int primSwap = info.IndexPrim0;
                info.IndexPrim0 = info.IndexPrim1;
                info.IndexPrim1 = primSwap;
            }

            Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
            Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;

            Box oldBox = (Box)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
            Box newBox = (Box)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);

            JigLibX.Geometry.Plane oldPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
            JigLibX.Geometry.Plane newPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);

            // quick check
            float centreDist = Distance.PointPlaneDistance(newBox.GetCentre(), newPlane);
            if (centreDist > collTolerance + newBox.GetBoundingRadiusAroundCentre())
                return;

            Vector3[] newPts = new Vector3[8];
            newBox.GetCornerPoints(out newPts);
            Vector3[] oldPts = new Vector3[8];
            oldBox.GetCornerPoints(out oldPts);

            List<CollPointInfo> collPts = new List<CollPointInfo>(8);

            for (int i = 0; i < 8; ++i)
            {
                Vector3 newPt = newPts[i];
                Vector3 oldPt = oldPts[i];
                float oldDepth = -Distance.PointPlaneDistance(oldPt, oldPlane);
                float newDepth = -Distance.PointPlaneDistance(newPt, newPlane);

                if (MathHelper.Max(oldDepth, newDepth) > -collTolerance)
                    collPts.Add(new CollPointInfo(oldPt - body0Pos, oldPt - body1Pos, oldDepth));
            }

            if (collPts.Count > 0)
            {
                collisionFunctor.CollisionNotify(info, oldPlane.Normal, collPts);
            }

        }



    }
}

